%% 求解线性定标不等式（LDI）的最大终端管（terminal tube）来设计线性状态反馈控制器
% Q和R是代价函数的权重矩阵，ucon和xcon是输入和状态的约束范围，alpha是终端管（terminal tube）的初始大小
% 函数的输出是终端区域的参数：P是一个正定矩阵，K是线性状态反馈矩阵，alpha是终端区域的大小

function [P, K, alpha] = NMPC_get_max_terminal_tube_new (Q, R, ucon, xcon, alpha, A, B)
%% Get the maximal terminal region 
% Method adopted from Wenhua Chen et al. 2003 in IJACSP. By Zehua Jia, jiazehua@sjtu.edu.cn

%% Input
% ucon: |u| <= ucon; 
% xcon: |x| <= xcon
% Here x(i) has the same upper and lower bounds, i = 1, 2.
% alpha: x' * P * x <= alpha

%% System description
% x' = A * x + B * u + Bw * w
% The designed local state-feeback control law is u = K * x.

%% Some Tips
% 确保在使用YALMIP进行凸优化建模时，问题能够被正确地表示为凸问题，从而能够有效地使用凸优化工具求解
% -logdet(X)表示矩阵X的行列式（determinant）的负对数，这种形式在一些凸优化问题中很常见

% A. The constraints should be Linear, which means nonlinear terms are not
% permitted (such as x*y and x^2, where x,y are both sdpvars).
% 在使用YALMIP进行优化建模时，约束应该是线性的，不能包含非线性项

% B. The inverse of a sdpvar is not desired in constraints or objective.
% logdet(X^(-1)) = -logdet(X).
% 在约束或目标函数中不应该使用sdpvar的逆，直接使用逆的话可能会导致非凸问题

% C. logdet is concave, and then -logdet is convex.
% 在凸优化中，凸函数具有良好的性质

% D. One should always try to reformulate the problem to obtain a convex problem. 
% 始终尝试通过重新表述问题来获得凸问题。这是一种通用的凸优化的建模原则。凸问题通常更易于求解，并且具有全局最优解。

% E. -logdet(a * X) (sdpvar a, X = sdpvar(n,n)) can be reformulated as 
% -logdet(Y) (sdpvar a, Y = sdpvar(n,n)). Then multiply LMI constraints by 
% a in both sides, and replace Y = a * X in constraints, 
% 建议对-logdet(a * X)进行重新表述，可以更容易地处理凸优化问题，并且避免了直接使用逆矩阵的问题

%% Initiallization
umax = ucon;
umin = -ucon;
xmax = xcon;
xmin = -xcon;

%% LDI approximation within the selected set x <= 2
% 进行线性定标不等式（LDI）的近似，以构建在给定集合中的线性系统动态模型
% 创建多个近似线性系统模型，每个模型在给定的状态集合（在此处是 x <= 2）内是有效的。
% 常用于将非线性动态系统近似为一系列线性动态系统

% 引入非线性部分 g(x)，其中xmax是先前定义的状态变量上限
g = [0, 0, 0; 0, 0, 0; 0, 0, 0]; % 不包含非线性部分
% 构建增广矩阵F，其中包括线性和非线性部分
F = [A + g, B];

%% Solve the optimization problem using YALMIP

% 将状态和输入的约束转化为标准形式
c(1, :) = [1/xmax(1), 0, 0];   % for x1 < 2
c(2, :) = [0, 1/xmax(2), 0];   % for x2 < 2
c(3, :) = [0, 0, 1/xmax(3)];   % for x3 < 2
c(4, :) = -c(1, :);            % for x1 > -2
c(5, :) = -c(2, :);            % for x2 > -2
c(6, :) = -c(3, :);            % for x3 > -2
c(7, :) = zeros(1, 3);
c(8, :) = zeros(1, 3);
% 输入变量u的约束
d(1, :) = [0, 0];
d(2, :) = [0, 0];
d(3, :) = [0, 0];
d(4, :) = [0, 0];
d(5, :) = [0, 0];
d(6, :) = [0, 0];
d(7, :) = [1/umax(1), 1/umax(2)]; % for u1 < 2, u2 < 2
d(8, :) = -d(7, :);               % for u1 > -2, u2 > -2

[Nc, ~] = size(d); % 6 Number of constraints

% Define decision matrix variables
[n,~] = size(Q); % 3
[~,m] = size(R); % 2
% 定义决策变量。这些变量包括W1​、W2​和α0​
yalmip('clear');
alpha0 = sdpvar(1);
W1 = sdpvar(n,n); % W1 = alpha * W1 (The latter W1 is the W1 in the paper)
W2 = sdpvar(m,n,'full'); % W2 = alpha * W2 (The latter W2 is the W2 in the paper)
W = [W1, W2'];
MAT1 = sdpvar(2*n+m, 2*n+m);
MAT2 = sdpvar(1+n, 1+n, Nc);

% Define LMI constraints
% 约束用于确保一组矩阵（在这里是W1​和W2​）满足一定条件
% 构建关于系统动态的约束 / 构建关于控制器和性能权衡的约束 / 关于权衡性能的约束
MAT1(:,:) = [-F * W' - W * F', [W1 * Q^(0.5), W2']; [W1 * Q^(0.5), W2']', alpha0 * [eye(3), zeros(3,2); zeros(2,3), R^(-1)]]; % Multiplying alpha in both sides in LMI (19)

for i = 1:Nc
    % 构建关于系统状态和输入变量的约束
    MAT2(:,:,i) = [1, c(i, :) * W1 + d(i,:) * W2; (c(i, :) * W1 + d(i,:) * W2)', W1]; % Multiplying alpha in both sides in LMI (20)
end

const = [];
% 表示矩阵MAT1(:,:,i)的每个元素都应大于等于零，用于确保矩阵MAT1(:,:,i)是半正定的
% const = [const, MAT1(:,:)>=0];

% debug 2024/01/22
const = [const, MAT1(:,:)>=-10]; %>=-10

for i = 1:Nc
    const = [const, MAT2(:,:,i)>=0];
end

% const = [const, alpha <= 10]; % Without this constraint, the solver will turn out error "lack of progress"
% 目标函数定义，det表示矩阵的行列式，最大化−logdet(W1)等同于最小化logdet(W1​)
% 找到一组优化变量W1和W2，使得满足线性矩阵不等式，并最大化-logdet(W1)的值
obj = -logdet(W1);
% 汇总所有约束，加入额外的约束，α0​≤α
const1 = [const, alpha0 <= alpha]; % The LMI method with limited alpha

%% Solving
% 使用YALMIP的optimize函数解决优化问题，找到W1​、W2​和α0的值，满足定义的约束条件，同时最小化目标函数
ops = sdpsettings('solver','Mosek','verbose', 0); % fmincon不可以/sdpt3/Mosek
if alpha == -1
    optimize(const, obj, ops); % When input alpha == -1, it means no selected alpha
elseif alpha <= 0
    error('The alphaM must be larger than 0')
else  
    optimize(const1, obj, ops);
end

%% Get results
% 解决优化问题后，获取最优解
W1 = double(W1) / double(alpha0);
W2 = double(W2) / double(alpha0);
P = W1^(-1); % X^(-1)
K = W2 * W1^(-1); % Y*X^(-1)
alpha = double(alpha0); % α

%% Plot the obtained ellipsoid (terminal region)
% draw_ellip(P, alpha, 'k')
% hold on % Used for multiple eliipses comparison
end
